#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/opencv.hpp>
#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <linux/fb.h>
#include <sys/mman.h>
#include <math.h>
#include<sys/ioctl.h>

using namespace std;

std::string gstreamer_pipeline (int capture_width, int capture_height, int display_width, int display_height, int framerate, int flip_method)
{
    return "nvarguscamerasrc sensor-id=0 ! video/x-raw(memory:NVMM), width=(int)" + std::to_string(capture_width) + ", height=(int)" +
           std::to_string(capture_height) + ", format=(string)NV12, framerate=(fraction)" + std::to_string(framerate) +
           "/1 ! nvvidconv flip-method=" + std::to_string(flip_method) + " ! video/x-raw, width=(int)" + std::to_string(display_width) + ", height=(int)" +
           std::to_string(display_height) + ", format=(string)BGRx ! autovideoconvert ! video/x-raw, format=(string)BGR ! appsink";//appsink
}

int main(int argc, char **argv)
{
    int w,h;
    int fd;
    struct fb_var_screeninfo fb_var;
    fd = open("/dev/fb0",O_RDWR);
    ioctl (fd,FBIOGET_VSCREENINFO,&fb_var);
    w = fb_var.xres;
    h = fb_var.yres;

    ros::init(argc, argv, "pub_cam_node");
    ros::NodeHandle n;

    int capture_width = 1280 ;
    int capture_height = 720 ;
    //int display_width = 1280 ;
    //int display_height = 720 ;ros
    int display_width = w ;
    int display_height = h;
    int framerate = 10 ;
    int flip_method =0 ;

    std::string pipeline = gstreamer_pipeline(capture_width,
                           capture_height,
                           display_width,
                           display_height,
                           framerate,
                           flip_method);
    cout << "Using pipeline: \n\t" << pipeline << endl;

    //1 捕获视频
    cv::VideoCapture capture;
    capture.open(pipeline, cv::CAP_GSTREAMER);
    //capture.open(1);
    if(!capture.isOpened())
    {
        ROS_ERROR_STREAM("Failed to open camera!");
        ros::shutdown();
    }

    //2 创建ROS中图像的发布者
    image_transport::ImageTransport it(n);
    image_transport::Publisher pub_image = it.advertise("/camera_csi0/frames", 1);

    //cv_bridge功能包提供了ROS图像和OpenCV图像转换的接口，建立了一座桥梁
    cv_bridge::CvImagePtr frame = boost::make_shared<cv_bridge::CvImage>();
    frame->encoding = sensor_msgs::image_encodings::BGR8;
    ros::Rate loop_rate(100);

    while(ros::ok())
    {
        capture >> frame->image; //流的转换

        if(frame->image.empty())
        {
            ROS_ERROR_STREAM("Failed to capture frame!");
            ros::shutdown();
        }
        //打成ROS数据包
        frame->header.stamp = ros::Time::now();
        pub_image.publish(frame->toImageMsg());

        cv::waitKey(3);//opencv刷新图像 3ms
        ros::spinOnce();
        loop_rate.sleep();
    }

    capture.release();    //释放流
    return EXIT_SUCCESS;
}
